%% 连接到ROS master
%setenv("ROS_MASTER_URI","http://192.168.1.140:11311")
%rosinit; %只用刚开始启动一次，关闭用rosshutdown

%% 发布ROS话题
velPub = rospublisher('/custom_velmsg','geometry_msgs/Twist');
velMsg = rosmessage(velPub);
odomSub = rossubscriber('/odom');

%% 订阅ROS话题
while true 
    odomMsg = receive(odomSub, 0.1); % 等待0.1秒钟接收消息
    
    pose = odomMsg.Pose;
    position = pose.Pose.Position; % 获取位置信息
    orientation = pose.Pose.Orientation; % 获取姿态信息
    
    pos = [position.X, position.Y];% 提取位置信息
    quat = [orientation.X, orientation.Y, orientation.Z, orientation.W];% 提取姿态信息
    eulerAngles = quat2eul(quat);% 将四元数表示的姿态转换为欧拉角
    theta = eulerAngles(3); % 提取yaw角度作为朝向角度theta
    poseInfo = [pos, theta]; % 最终位置姿态信息
   
    % 计算速度信息（假设这里是根据位置信息进行计算）
    % 这里仅作为示例，实际应用中可能需要根据实际情况进行计算
    linearVelocity = 0.0; % 线速度
    angularVelocity = 0.0; % 角速度
    pause(0.5)

    % 将速度信息填充到 ROS 消息中
    velMsg.Linear.X = linearVelocity;
    velMsg.Angular.Z = angularVelocity;

    % 发布速度信息到自定义话题
    send(velPub, velMsg);

    % 清除odomSub订阅器
    %clear odomSub;
end